#include "connection.h"
#include <iostream>
using namespace std;

connection::connection(){

        ip="224.5.23.2";
        port=20011;
        timeStamp=0;
        wheel1=0;
        wheel2=0;
        wheel3=0;
        wheel4=0;
        kickspeedx=0;
        kickspeedz=0;
        velx=0;
        vely=0;
        velz=0;
        id_=0;
        spinner=false;
        teamYellow=false;
}

connection::~connection(){
}
void connection::setTimeStamp(float time){
    timeStamp = time;
}
void connection::setWheel1(float wheel){
    wheel1= wheel;
}

void connection::setWheel2(float wheel){
    wheel2 = wheel;
}

void connection::setWheel3(float wheel){
    wheel3 = wheel;
}

void connection::setWheel4(float wheel){
    wheel4 = wheel;
}

void connection::setKickSpeedx(float kick){
    kickspeedx = kick;
}
void connection::setKickSpeedz(float kick){
    kickspeedz = kick;
}
void connection::setvelx(float vel){
    velx = vel;
}
void connection::setvely(float vel){
    vely = vel;
}
void connection::setvelz(float vel){
    velz = vel;
}
void connection::setspin(bool spin){
    spinner = spin;
}
void connection::setteam(bool team){
    teamYellow = team;
}
void connection::setip(string ip_){
    ip = ip_;
}
void connection::setport(int port_){
    port = port_;
}
void connection::setid(int id_aux){

    id_ = id_aux;

}
bool connection::conectar(){

    udpsocket.close();
    bool flag = _addr.setHost(ip.c_str(),port);
    if (udpsocket.open(port, false, false, false) && flag) return true;
    else return false;

}
void connection::desconectar(){
    udpsocket.close();
}
bool connection::isconect(){
    return udpsocket.isOpen();
}

void connection::send(){


    grSim_Packet packet;

    packet.mutable_commands()->set_isteamyellow(teamYellow);
    packet.mutable_commands()->set_timestamp(timeStamp);
    grSim_Robot_Command* command = packet.mutable_commands()->add_robot_commands();
    command->set_id(id_);

    command->set_wheelsspeed(true);

    command->set_wheel1(wheel1);
    command->set_wheel2(wheel2);
    command->set_wheel3(wheel3);
    command->set_wheel4(wheel4);
    command->set_veltangent(velx);
    command->set_velnormal(vely);
    command->set_velangular(velz);

    command->set_kickspeedx(kickspeedx);
    command->set_kickspeedz(kickspeedz);
    command->set_spinner(spinner);

    string s;
    packet.SerializeToString(&s);


    udpsocket.send((void*) s.c_str(), s.length(), _addr);


}
